#include "user_callback.h"
#include "FreeRTOS.h"
#include "task.h"
#include "event_groups.h"
#include "usart_interface.h"
#include "user_global.h"
#include "can_interface.h"
#include "can_protocol.h"

uint16_t Clear_IT;
extern EventGroupHandle_t USARTHandler;

#define DETECT_MONITOR(name) G_system_monitor.name##_fps = \
G_system_monitor.name##_cnt; G_system_monitor.name##_cnt = 0

#define DETECT_RS_MONITOR(name) G_referee_monitor.name##_fps = \
G_referee_monitor.name##_cnt; G_referee_monitor.name##_cnt = 0

extern "C"{
    extern void xPortSysTickHandler(void);
}

void ParseUSBData(uint8_t *buffer, uint32_t length) {
    static uint8_t state = 0;
    static uint8_t staticBuff[128];
    static uint32_t staticBuffIndex = 0;
    unsigned char returnFlag = 0;

    for (uint32_t i = 0; i < length; i++) {
        uint8_t byte = buffer[i];

        switch (state) {
            case 0:
                staticBuffIndex = 0;
                if (byte == 0x55) {
                    state = 1;
                    staticBuff[staticBuffIndex++] = byte;
                }
                break;

            case 1:
                if (byte == 0x00) {
                    state = 2;
                    staticBuff[staticBuffIndex++] = byte;
                } else {
                    state = 0;
                }
                break;

            case 2:
                if (byte == 0x01) {  // ID
                    state = 3;
                    staticBuff[staticBuffIndex++] = byte;
                } else {
                    state = 0;
                }
                break;

            case 3:
                if (byte == 22) {  // 数据长度
                    state = 4;
                    staticBuff[staticBuffIndex++] = byte;
                } else {
                    state = 0;
                }
                break;

            case 4:
                if (staticBuffIndex < 22 * 4 + 4) {
                    staticBuff[staticBuffIndex++] = byte;
                    if (staticBuffIndex == 22 * 4 + 4) {
                        state = 5;
                    }
                } else {
                    state = 0;
                }
                break;

            case 5:
                if (staticBuffIndex < 22 * 4 + 4 + 7) {
                    staticBuff[staticBuffIndex++] = byte;
                    if (staticBuffIndex == 22 * 4 + 4 + 7) {
                        state = 6;
                    }
                } else {
                    state = 0;
                }
                break;

            case 6:
                if (byte == 0x00) {
                    state = 7;
                    staticBuff[staticBuffIndex++] = byte;
                } else {
                    state = 0;
                }
                break;

            case 7:
                if (byte == 0xAA) {
                    staticBuff[staticBuffIndex++] = byte;

                        // 发送解析成功的数据到串口
                        UART_NavigationDataDecode(staticBuff);
									state = 0;
                    }
                    
                 else {
                    state = 0;
                }
                break;

            default:
                state = 0;
                break;
        }
    }
}



void User_SysTickCallback(void){
    if(xTaskGetSchedulerState()!=taskSCHEDULER_NOT_STARTED){
        xPortSysTickHandler();	
    }

    if(G_system_monitor.SysTickTime % 1000 == 0) {
		// Main Task Monitor
		DETECT_MONITOR(DataVisualTask);
		DETECT_MONITOR(SystemMonitorTask);
		DETECT_MONITOR(RobotControlTask);
		DETECT_MONITOR(NavigationDataSendTask);
		DETECT_MONITOR(GimbalDataSendTask);
		DETECT_MONITOR(AimAssistDataSendTask);
		DETECT_MONITOR(USBAimAssistDataSendTask);
		DETECT_MONITOR(RefereeDataSendTask);
		DETECT_MONITOR(Dijrc_Receive);
		DETECT_MONITOR(Referee_Receive);
		DETECT_MONITOR(Vofa_Receive);
		DETECT_MONITOR(Gimbal_Receive);
		DETECT_MONITOR(Navigation_Receive);
		DETECT_MONITOR(Aimassist_Receive);


		// IT Monitor
		DETECT_MONITOR(UART1_rx);
		DETECT_MONITOR(UART2_rx);
		DETECT_MONITOR(UART3_rx);
		DETECT_MONITOR(UART4_rx);
		DETECT_MONITOR(UART5_rx);
		DETECT_MONITOR(UART6_rx);
		DETECT_MONITOR(CAN1_rx);
		DETECT_MONITOR(CAN2_rx);

		// Referee Monitor
		DETECT_RS_MONITOR(GameStatus);
		DETECT_RS_MONITOR(GameRobotHP);
		DETECT_RS_MONITOR(DartStatus);
		DETECT_RS_MONITOR(EventData);
		DETECT_RS_MONITOR(RefereeWarning);
		DETECT_RS_MONITOR(DartRemainingTime);
		DETECT_RS_MONITOR(GameRobotStatus);
		DETECT_RS_MONITOR(PowerHeatData);
		DETECT_RS_MONITOR(GameRobotPos);
		DETECT_RS_MONITOR(Buff);
		DETECT_RS_MONITOR(RobotHurt);
		DETECT_RS_MONITOR(ShootData);
		DETECT_RS_MONITOR(BulletRemaining);
		DETECT_RS_MONITOR(RFIDStatus);
		DETECT_RS_MONITOR(RobotInteractiveData);
		DETECT_RS_MONITOR(RobotCommand);

		G_system_monitor.SysFault.CAN1_fault = G_system_monitor.CAN1_rx_fps < 3500;
		G_system_monitor.SysFault.CAN2_fault = G_system_monitor.CAN1_rx_fps < 3500;
		G_system_monitor.SysFault.GimbalCAN1_fault = G_system_monitor.Gimbal_CAN1_fps < 3500 && G_system_monitor.Gimbal_Receive_fps > 800;
		G_system_monitor.SysFault.GimbalCAN2_fault = G_system_monitor.Gimbal_CAN2_fps < 3500 && G_system_monitor.Gimbal_Receive_fps > 800;
		G_system_monitor.SysFault.Gimbal2ChassisCom_fault = G_system_monitor.Gimbal_Receive_fps < 800;
		G_system_monitor.SysFault.Chassis2GimbalCom_fault = G_system_monitor.Gimbal_Chassis_Receive_fps < 800 && G_system_monitor.Gimbal_Receive_fps > 800;
		G_system_monitor.SysFault.Yaw2GimbalCom_fault = G_system_monitor.Gimbal_Yaw_Receive_fps < 800 && G_system_monitor.Gimbal_Receive_fps > 800;
		G_system_monitor.SysFault.NavigationCom_fault = G_system_monitor.Navigation_Receive_fps < 50;
		G_system_monitor.SysFault.AimassistCom_fault = G_system_monitor.Aimassist_Receive_fps < 50;
		G_system_monitor.SysFault.RefereeCom_fault = G_system_monitor.Referee_Receive_fps < 20;
	}

	G_system_monitor.SysTickTime++;
}


void User_UART_RX_Callback(USART_TypeDef* USARTx){
	Usart* Usartx;
	BaseType_t Result,xHigherPriorityTaskWoken;

	if(G_djirc.usart_djirc->USART_RT.USARTx == USARTx){
		Usartx = G_djirc.usart_djirc;
	}
	else if (G_referee.usart_referee->USART_RT.USARTx == USARTx){
		Usartx = G_referee.usart_referee;
	}
	else if (G_vofa.usart_vofa->USART_RT.USARTx == USARTx){
		Usartx = G_vofa.usart_vofa;
	}
	else if (G_gimbal.usart_gimbal->USART_RT.USARTx == USARTx){
		Usartx = G_gimbal.usart_gimbal;
	}
	// else if (G_navigation.usart_navigation->USART_RT.USARTx == USARTx){
	// 	Usartx = G_navigation.usart_navigation;
	// }
	else if (G_aim_assist.usart_assist->USART_RT.USARTx == USARTx){
		Usartx = G_aim_assist.usart_assist;
	}

	User_UART_Fps_Update(USARTx);

	if(USART_GetITStatus(Usartx->USART_RT.USARTx, USART_IT_IDLE)!= RESET){
		Clear_IT = Usartx->USART_RT.USARTx->SR;
		Clear_IT = Usartx->USART_RT.USARTx->DR;//先读SR后读DR清楚中断标志位

		u8 datam = USART_ReceiveData(UART5);

		if(G_referee.usart_referee->USART_RT.USARTx == USARTx){
			Usartx->USART_Receive();
			if(USARTHandler != NULL){
				Result = xEventGroupSetBitsFromISR(USARTHandler,Usartx->USART_RT.Usart_event_bit,&xHigherPriorityTaskWoken);
				if(Result != pdFALSE){
					portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
				}
			}
		}
		else{
			if(Usartx->USART_Receive() == Usartx->USART_RT.MbLen){
				if(USARTHandler != NULL){
					Result = xEventGroupSetBitsFromISR(USARTHandler,Usartx->USART_RT.Usart_event_bit,&xHigherPriorityTaskWoken);
					if(Result != pdFALSE){
						portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
					}
				}
			}
		}	
	}
}


void User_UART_Fps_Update(USART_TypeDef* USARTx){
	if(USARTx == USART1)	G_system_monitor.UART1_rx_cnt++;
	else if(USARTx == USART2)	G_system_monitor.UART2_rx_cnt++;
	else if(USARTx == USART3)	G_system_monitor.UART3_rx_cnt++;
	else if(USARTx == UART4)	G_system_monitor.UART4_rx_cnt++;
	else if(USARTx == UART5)	G_system_monitor.UART5_rx_cnt++;
	else if(USARTx == USART6)	G_system_monitor.UART6_rx_cnt++;
}

void usb_count(void){
	G_system_monitor.Navigation_Receive_cnt++;
}


void User_CAN_RX_Callback(CAN_TypeDef * hCANx){
	CANContext* canx_context;
	BaseType_t Result,xHigherPriorityTaskWoken;

	if(hCANx == CAN1)	canx_context = &can1_context;
	else if(hCANx == CAN2)	canx_context = &can2_context;

	if(CAN_GetITStatus(hCANx,CAN_IT_FMP0)!= RESET){
		CAN_ClearITPendingBit(hCANx, CAN_IT_FMP0);
		CAN_Receive(hCANx, CAN_FIFO0, &canx_context->CANx_RxMsg);

		CAN_Decode(canx_context);

		if(hCANx == CAN1)	G_system_monitor.CAN1_rx_cnt++;
		else if(hCANx == CAN2)	G_system_monitor.CAN2_rx_cnt++;
	}
}